import rclpy
from rclpy.node import Node
from topic_msg.msg import CatStatus
from topic_msg.msg import CatSteer
from topic_msg.msg import CatHearing
from topic_msg.msg import CatDeriction
import threading
import time
import random
from .leg import Leg
from .leg import Action


class CatLegsNode(Node):
    def __init__(self, name):
        super().__init__(name)
        self._leg_publisher = self.create_publisher(CatSteer, "cat_steers", 10)
        time.sleep(5)
        self._cat_status_lock = threading.Lock()
        self._cat_status = CatStatus.CAT_ACTIVE
        self._action_lock = threading.Lock()
        self._actions = []
        self._pose_lock = threading.Lock()
        self._pose = Action.GET_DOWN
        self._legs_lock = threading.Lock()
        self._cat_deriction = CatDeriction.HORIZONTAL
        self._cat_deriction_lock = threading.Lock()

        self._legs = [
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
        ]
        self._set_cat_legs_angle()
        time.sleep(1)

        self._get_down()

        self._hearing_subscriper = self.create_subscription(
            CatHearing, "cat_hearing", self._rcv_hearing_callback, 10
        )

        self._cat_status_subscriper = self.create_subscription(
            CatStatus, "cat_status", self._rcv_cat_status_callback, 10
        )

        self._cat_deriction_subscriper = self.create_subscription(
            CatDeriction, "cat_deriction", self._rcv_cat_deriction_callback, 10
        )

        self._thd_usual_action = threading.Thread(target=self._usual_action)
        self._thd_random_action = threading.Thread(target=self._random_action)

        self._thd_usual_action.start()
        self._thd_random_action.start()

    def __del__(self):
        self._thd_usual_action.join()
        self._thd_random_action.join()
        self._leg_publisher.destroy()
        self._hearing_subscriper.destroy()
        self._cat_status_subscriper.destroy()
        self._cat_deriction_subscriper.destroy()

    def _rcv_cat_deriction_callback(self, msg):
        # self.get_logger().info(f"msg:{msg.deriction}")
        tmp = CatDeriction.HORIZONTAL
        with self._cat_deriction_lock:
            tmp = self._cat_deriction
            self._cat_deriction = msg.deriction

        if tmp == CatDeriction.HORIZONTAL and msg.deriction == CatDeriction.VERTICAL:
            # self.get_logger().info(f"get_down")
            with self._action_lock:
                self._actions.clear()
                self._get_down()
        elif tmp == CatDeriction.VERTICAL and msg.deriction == CatDeriction.HORIZONTAL:
            # self.get_logger().info(f"stand_up")
            with self._action_lock:
                self._actions.clear()
                self._stand_up()

    def _rcv_cat_status_callback(self, msg):
        tmp = CatStatus.CAT_ACTIVE
        with self._cat_status_lock:
            tmp = self._cat_status
            self._cat_status = msg.status

        if (tmp == CatStatus.CAT_ACTIVE or tmp == CatStatus.CAT_DONT_ANOUNCE) and (
            msg.status == CatStatus.CAT_STOP or msg.status == CatStatus.CAT_DONT_MOVE
        ):
            with self._action_lock:
                self._actions.clear()
                self._sit_down()
        elif (
            msg.status == CatStatus.CAT_ACTIVE
            or msg.status == CatStatus.CAT_DONT_ANOUNCE
        ) and (tmp == CatStatus.CAT_STOP or tmp == CatStatus.CAT_DONT_MOVE):
            with self._action_lock:
                self._actions.clear()
                self._stand_up()

    def _rcv_hearing_callback(self, msg):
        # self.get_logger().info(f"msg:{msg.cmd}")
        with self._action_lock:
            if (
                msg.cmd == CatHearing.HELLO_1
                or msg.cmd == CatHearing.HELLO_2
                or msg.cmd == CatHearing.HELLO_3
            ):
                self._actions.clear()
                self._say_hello()
                return

            with self._cat_status_lock:
                if (
                    self._cat_status == CatStatus.CAT_DONT_MOVE
                    or self._cat_status == CatStatus.CAT_STOP
                ):
                    return
            tmp = CatDeriction.HORIZONTAL
            with self._cat_deriction_lock:
                tmp = self._cat_deriction

            if tmp == CatDeriction.HORIZONTAL:
                if (
                    msg.cmd == CatHearing.FORWARD_1
                    or msg.cmd == CatHearing.FORWARD_2
                    or msg.cmd == CatHearing.COME_OVER
                ):
                    self._actions.clear()
                    self._foreward(random.randint(3, 5))
                elif (
                    msg.cmd == CatHearing.RETURN
                    or msg.cmd == CatHearing.TURN_AROUND_1
                    or msg.cmd == CatHearing.TURN_AROUND_2
                ):
                    self._actions.clear()
                    self._turn_round()
                elif msg.cmd == CatHearing.RETREAT:
                    self._actions.clear()
                    self._retreat(random.randint(3, 5))
                elif msg.cmd == CatHearing.TURN_LEFT:
                    self._actions.clear()
                    self._turn_left()
                elif msg.cmd == CatHearing.TURN_RIGHT:
                    self._actions.clear()
                    self._turn_right()
                else:
                    pass
            if msg.cmd == CatHearing.SIT_DOWN:
                self._actions.clear()
                self._sit_down()
            elif msg.cmd == CatHearing.STAND_UP:
                self._actions.clear()
                self._stand_up()
            elif msg.cmd == CatHearing.GET_DOWN:
                self._actions.clear()
                self._get_down()
            else:
                pass

    def _say_hello(self):
        cur_legs = [
            Leg(Leg.STAND_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.STAND_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
        ]
        if self._get_current_pose() != Action.SITDOWN:
            self._sit_down()
        else:
            self._copy_legs(cur_legs, self._get_current_legs())

        dis_legs = [
            Leg(cur_legs[0].calf, cur_legs[0].thigh),
            Leg(Leg.SAY_HELLO_CALF_1, cur_legs[1].thigh),
            Leg(cur_legs[2].calf, cur_legs[2].thigh),
            Leg(cur_legs[3].calf, cur_legs[3].thigh),
        ]

        self._translate(dis_legs, cur_legs)

        self._copy_legs(cur_legs, dis_legs)

        dis_legs[0].calf = cur_legs[0].calf
        dis_legs[1].calf = cur_legs[1].calf
        dis_legs[2].calf = cur_legs[2].calf
        dis_legs[3].calf = cur_legs[3].calf

        dis_legs[0].thigh = cur_legs[0].thigh
        dis_legs[1].thigh = Leg.SAY_HELLO_THIGH
        dis_legs[2].thigh = cur_legs[2].thigh
        dis_legs[3].thigh = cur_legs[3].thigh

        self._translate(dis_legs, cur_legs)

        self._copy_legs(cur_legs, dis_legs)

        self._generate_wait_action(0.3)

        i = 0
        while i < 3:
            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = Leg.SAY_HELLO_CALF_2
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            self._generate_wait_action(0.3)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = Leg.SAY_HELLO_CALF_1
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            self._generate_wait_action(0.3)

            i += 1

        dis_legs[0].calf = Leg.STAND_CALF
        dis_legs[1].calf = Leg.STAND_CALF
        dis_legs[2].calf = Leg.GET_DOWN_CALF
        dis_legs[3].calf = Leg.GET_DOWN_CALF

        dis_legs[0].thigh = Leg.GET_DOWN_THIGH
        dis_legs[1].thigh = Leg.GET_DOWN_THIGH
        dis_legs[2].thigh = Leg.GET_DOWN_THIGH
        dis_legs[3].thigh = Leg.GET_DOWN_THIGH

        self._translate(dis_legs, cur_legs)

    def _foreward(self, step):
        cur_legs = [
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
        ]
        if self._get_current_pose != Action.STAND:
            self._stand_up()
        else:
            self._copy_legs(cur_legs, self._get_current_legs())

        dis_legs = [
            Leg(Leg.FORWARD_BEHIND_CALF_1, Leg.FORWARD_BEHIND_THIGH_1),
            Leg(Leg.FORWARD_FRONT_CALF_2, Leg.FORWARD_FRONT_THIGH_2),
            Leg(Leg.FORWARD_FRONT_CALF_3, Leg.FORWARD_FRONT_THIGH_3),
            Leg(Leg.FORWARD_BEHIND_CALF_4, Leg.FORWARD_BEHIND_THIGH_4),
        ]

        self._translate(dis_legs, cur_legs)

        self._copy_legs(cur_legs, dis_legs)

        i = 0
        while i < step:

            dis_legs[0].calf = Leg.FORWARD_FRONT_CALF_1 - 10

            dis_legs[1].calf = (
                Leg.FORWARD_BEHIND_CALF_2 + Leg.FORWARD_FRONT_CALF_2
            ) / 2

            dis_legs[2].calf = (
                Leg.FORWARD_BEHIND_CALF_3 + Leg.FORWARD_FRONT_CALF_3
            ) / 2

            dis_legs[3].calf = Leg.FORWARD_FRONT_CALF_4 - 10

            dis_legs[0].thigh = (
                Leg.FORWARD_FRONT_THIGH_1 + Leg.FORWARD_BEHIND_THIGH_1
            ) / 2
            dis_legs[1].thigh = (
                Leg.FORWARD_BEHIND_THIGH_2 + Leg.FORWARD_FRONT_THIGH_2
            ) / 2
            dis_legs[2].thigh = (
                Leg.FORWARD_BEHIND_THIGH_3 + Leg.FORWARD_FRONT_THIGH_3
            ) / 2
            dis_legs[3].thigh = (
                Leg.FORWARD_FRONT_THIGH_4 + Leg.FORWARD_BEHIND_THIGH_4
            ) / 2

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = Leg.FORWARD_FRONT_CALF_1
            dis_legs[1].calf = Leg.FORWARD_BEHIND_CALF_2
            dis_legs[2].calf = Leg.FORWARD_BEHIND_CALF_3
            dis_legs[3].calf = Leg.FORWARD_FRONT_CALF_4

            dis_legs[0].thigh = Leg.FORWARD_FRONT_THIGH_1
            dis_legs[1].thigh = Leg.FORWARD_BEHIND_THIGH_2
            dis_legs[2].thigh = Leg.FORWARD_BEHIND_THIGH_3
            dis_legs[3].thigh = Leg.FORWARD_FRONT_THIGH_4

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = (
                Leg.FORWARD_FRONT_CALF_1 + Leg.FORWARD_BEHIND_CALF_1
            ) / 2

            dis_legs[1].calf = Leg.FORWARD_FRONT_CALF_2 - 10

            dis_legs[2].calf = Leg.FORWARD_FRONT_CALF_3 - 10

            dis_legs[3].calf = (
                Leg.FORWARD_FRONT_CALF_4 + Leg.FORWARD_BEHIND_CALF_4
            ) / 2

            dis_legs[0].thigh = (
                Leg.FORWARD_FRONT_THIGH_1 + Leg.FORWARD_BEHIND_THIGH_1
            ) / 2
            dis_legs[1].thigh = (
                Leg.FORWARD_BEHIND_THIGH_2 + Leg.FORWARD_FRONT_THIGH_2
            ) / 2
            dis_legs[2].thigh = (
                Leg.FORWARD_BEHIND_THIGH_3 + Leg.FORWARD_FRONT_THIGH_3
            ) / 2
            dis_legs[3].thigh = (
                Leg.FORWARD_FRONT_THIGH_4 + Leg.FORWARD_BEHIND_THIGH_4
            ) / 2

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = Leg.FORWARD_BEHIND_CALF_1
            dis_legs[1].calf = Leg.FORWARD_FRONT_CALF_2
            dis_legs[2].calf = Leg.FORWARD_FRONT_CALF_3
            dis_legs[3].calf = Leg.FORWARD_BEHIND_CALF_4

            dis_legs[0].thigh = Leg.FORWARD_BEHIND_THIGH_1
            dis_legs[1].thigh = Leg.FORWARD_FRONT_THIGH_2
            dis_legs[2].thigh = Leg.FORWARD_FRONT_THIGH_3
            dis_legs[3].thigh = Leg.FORWARD_BEHIND_THIGH_4

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            i += 1
        self._quiet_action()

    def _retreat(self, step):
        cur_legs = [
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
        ]
        if self._get_current_pose != Action.STAND:
            self._stand_up()
        else:
            self._copy_legs(cur_legs, self._get_current_legs())

        dis_legs = [
            Leg(Leg.RETREAT_FRONT_CALF_1, Leg.RETREAT_FRONT_THIGH_1),
            Leg(Leg.RETREAT_BEHIND_CALF_2, Leg.RETREAT_BEHIND_THIGH_2),
            Leg(Leg.RETREAT_BEHIND_CALF_3, Leg.RETREAT_BEHIND_THIGH_3),
            Leg(Leg.RETREAT_FRONT_CALF_4, Leg.RETREAT_FRONT_THIGH_4),
        ]

        self._translate(dis_legs, cur_legs)

        self._copy_legs(cur_legs, dis_legs)

        i = 0
        while i < step:
            dis_legs[0].calf = Leg.RETREAT_FRONT_CALF_1 - 15

            dis_legs[1].calf = (
                Leg.RETREAT_FRONT_CALF_2 + Leg.RETREAT_BEHIND_CALF_2
            ) / 2

            dis_legs[2].calf = (
                Leg.RETREAT_FRONT_CALF_3 + Leg.RETREAT_BEHIND_CALF_3
            ) / 2

            dis_legs[3].calf = Leg.RETREAT_FRONT_CALF_4 - 15

            dis_legs[0].thigh = (
                Leg.RETREAT_FRONT_THIGH_1 + Leg.RETREAT_BEHIND_THIGH_1
            ) / 2
            dis_legs[1].thigh = (
                Leg.RETREAT_BEHIND_THIGH_2 + Leg.RETREAT_FRONT_THIGH_2
            ) / 2
            dis_legs[2].thigh = (
                Leg.RETREAT_BEHIND_THIGH_3 + Leg.RETREAT_FRONT_THIGH_3
            ) / 2
            dis_legs[3].thigh = (
                Leg.RETREAT_FRONT_THIGH_4 + Leg.RETREAT_BEHIND_THIGH_4
            ) / 2

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = Leg.RETREAT_BEHIND_CALF_1
            dis_legs[1].calf = Leg.RETREAT_FRONT_CALF_2
            dis_legs[2].calf = Leg.RETREAT_FRONT_CALF_3
            dis_legs[3].calf = Leg.RETREAT_BEHIND_CALF_4

            dis_legs[0].thigh = Leg.RETREAT_BEHIND_THIGH_1
            dis_legs[1].thigh = Leg.RETREAT_FRONT_THIGH_2
            dis_legs[2].thigh = Leg.RETREAT_FRONT_THIGH_3
            dis_legs[3].thigh = Leg.RETREAT_BEHIND_THIGH_4

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = (
                Leg.RETREAT_FRONT_CALF_1 + Leg.RETREAT_BEHIND_CALF_1
            ) / 2

            dis_legs[1].calf = Leg.RETREAT_FRONT_CALF_2 - 15

            dis_legs[2].calf = Leg.RETREAT_FRONT_CALF_3 - 15

            dis_legs[3].calf = (
                Leg.RETREAT_FRONT_CALF_4 + Leg.RETREAT_BEHIND_CALF_4
            ) / 2

            dis_legs[0].thigh = (
                Leg.RETREAT_FRONT_THIGH_1 + Leg.RETREAT_BEHIND_THIGH_1
            ) / 2
            dis_legs[1].thigh = (
                Leg.RETREAT_BEHIND_THIGH_2 + Leg.RETREAT_FRONT_THIGH_2
            ) / 2
            dis_legs[2].thigh = (
                Leg.RETREAT_BEHIND_THIGH_3 + Leg.RETREAT_FRONT_THIGH_3
            ) / 2
            dis_legs[3].thigh = (
                Leg.RETREAT_FRONT_THIGH_4 + Leg.RETREAT_BEHIND_THIGH_4
            ) / 2

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = Leg.RETREAT_FRONT_CALF_1
            dis_legs[1].calf = Leg.RETREAT_BEHIND_CALF_2
            dis_legs[2].calf = Leg.RETREAT_BEHIND_CALF_3
            dis_legs[3].calf = Leg.RETREAT_FRONT_CALF_4

            dis_legs[0].thigh = Leg.RETREAT_FRONT_THIGH_1
            dis_legs[1].thigh = Leg.RETREAT_BEHIND_THIGH_2
            dis_legs[2].thigh = Leg.RETREAT_BEHIND_THIGH_3
            dis_legs[3].thigh = Leg.RETREAT_FRONT_THIGH_4

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            i += 1

        self._quiet_action()

    def _turn_right(self):
        cur_legs = [
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
        ]
        if self._get_current_pose != Action.STAND:
            self._stand_up()
        else:
            self._copy_legs(cur_legs, self._get_current_legs())

        dis_legs = [
            Leg(Leg.FORWARD_BEHIND_CALF_1, Leg.FORWARD_BEHIND_THIGH_1),
            Leg(Leg.FORWARD_FRONT_CALF_2, Leg.FORWARD_FRONT_THIGH_2),
            Leg(Leg.FORWARD_BEHIND_CALF_3, Leg.FORWARD_BEHIND_THIGH_3),
            Leg(Leg.FORWARD_FRONT_CALF_4, Leg.FORWARD_FRONT_THIGH_4),
        ]

        self._translate(dis_legs, cur_legs)

        self._copy_legs(cur_legs, dis_legs)

        step = random.randint(3, 15)
        i = 0
        while i < step:
            dis_legs[0].calf = Leg.FORWARD_BEHIND_CALF_1 - 20
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = (
                Leg.FORWARD_FRONT_THIGH_1 + Leg.FORWARD_BEHIND_THIGH_1
            ) / 2
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = Leg.FORWARD_FRONT_CALF_1
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = Leg.FORWARD_FRONT_THIGH_1
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = Leg.FORWARD_BEHIND_CALF_3 - 20
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = (
                Leg.FORWARD_FRONT_THIGH_3 + Leg.FORWARD_BEHIND_THIGH_3
            ) / 2
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = Leg.FORWARD_FRONT_CALF_3
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = Leg.FORWARD_FRONT_THIGH_3
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = Leg.FORWARD_FRONT_CALF_2 - 20
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = (
                Leg.FORWARD_FRONT_THIGH_2 + Leg.FORWARD_BEHIND_THIGH_2
            ) / 2
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = Leg.FORWARD_BEHIND_CALF_2
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = Leg.FORWARD_BEHIND_THIGH_2
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = Leg.FORWARD_FRONT_CALF_4 - 20

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = (
                Leg.FORWARD_FRONT_THIGH_4 + Leg.FORWARD_BEHIND_THIGH_4
            ) / 2

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = Leg.FORWARD_BEHIND_CALF_4

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = Leg.FORWARD_BEHIND_THIGH_4

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = Leg.FORWARD_BEHIND_CALF_1
            dis_legs[1].calf = Leg.FORWARD_FRONT_CALF_2
            dis_legs[2].calf = Leg.FORWARD_BEHIND_CALF_3
            dis_legs[3].calf = Leg.FORWARD_FRONT_CALF_4

            dis_legs[0].thigh = Leg.FORWARD_BEHIND_THIGH_1
            dis_legs[1].thigh = Leg.FORWARD_FRONT_THIGH_2
            dis_legs[2].thigh = Leg.FORWARD_BEHIND_THIGH_3
            dis_legs[3].thigh = Leg.FORWARD_FRONT_THIGH_4

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            i += 1
        self._quiet_action()

    def _turn_left(self):
        cur_legs = [
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
            Leg(Leg.STAND_CALF, Leg.STAND_THIGH),
        ]
        if self._get_current_pose != Action.STAND:
            self._stand_up()
        else:
            self._copy_legs(cur_legs, self._get_current_legs())

        dis_legs = [
            Leg(Leg.RETREAT_FRONT_CALF_1, Leg.RETREAT_FRONT_THIGH_1),
            Leg(Leg.RETREAT_BEHIND_CALF_2, Leg.RETREAT_BEHIND_THIGH_2),
            Leg(Leg.RETREAT_FRONT_CALF_3, Leg.RETREAT_FRONT_THIGH_3),
            Leg(Leg.RETREAT_BEHIND_CALF_4, Leg.RETREAT_BEHIND_THIGH_4),
        ]

        self._translate(dis_legs, cur_legs)

        self._copy_legs(cur_legs, dis_legs)

        step = random.randint(3, 15)
        i = 0
        while i < step:
            dis_legs[0].calf = Leg.RETREAT_FRONT_CALF_1 - 10
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = (
                Leg.RETREAT_FRONT_THIGH_1 + Leg.RETREAT_BEHIND_THIGH_1
            ) / 2
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = Leg.RETREAT_BEHIND_CALF_1
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = Leg.RETREAT_BEHIND_THIGH_1
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = Leg.RETREAT_FRONT_CALF_3 - 10
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = (
                Leg.RETREAT_FRONT_THIGH_3 + Leg.RETREAT_BEHIND_THIGH_3
            ) / 2
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = Leg.RETREAT_BEHIND_CALF_3
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = Leg.RETREAT_BEHIND_THIGH_3
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = Leg.RETREAT_BEHIND_CALF_2 - 10
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = (
                Leg.RETREAT_FRONT_THIGH_2 + Leg.RETREAT_BEHIND_THIGH_2
            ) / 2
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = Leg.RETREAT_FRONT_CALF_2
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = cur_legs[3].calf

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = Leg.RETREAT_FRONT_THIGH_2
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = cur_legs[3].thigh

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = Leg.RETREAT_BEHIND_CALF_4 - 20

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = (
                Leg.RETREAT_FRONT_THIGH_4 + Leg.RETREAT_BEHIND_THIGH_4
            ) / 2

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = cur_legs[0].calf
            dis_legs[1].calf = cur_legs[1].calf
            dis_legs[2].calf = cur_legs[2].calf
            dis_legs[3].calf = Leg.RETREAT_FRONT_CALF_4

            dis_legs[0].thigh = cur_legs[0].thigh
            dis_legs[1].thigh = cur_legs[1].thigh
            dis_legs[2].thigh = cur_legs[2].thigh
            dis_legs[3].thigh = Leg.RETREAT_FRONT_THIGH_4

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            dis_legs[0].calf = Leg.RETREAT_FRONT_CALF_1
            dis_legs[1].calf = Leg.RETREAT_BEHIND_CALF_2
            dis_legs[2].calf = Leg.RETREAT_FRONT_CALF_3
            dis_legs[3].calf = Leg.RETREAT_BEHIND_CALF_4

            dis_legs[0].thigh = Leg.RETREAT_FRONT_THIGH_1
            dis_legs[1].thigh = Leg.RETREAT_BEHIND_THIGH_2
            dis_legs[2].thigh = Leg.RETREAT_FRONT_THIGH_3
            dis_legs[3].thigh = Leg.RETREAT_BEHIND_THIGH_4

            self._translate(dis_legs, cur_legs)

            self._copy_legs(cur_legs, dis_legs)

            i += 1
        self._quiet_action()

    def _turn_round(self):
        pass

    def _sit_down(self):

        self._generate_pose_action(Action.SITDOWN)

        cur_legs = self._get_current_legs()

        dis_legs = [
            Leg(cur_legs[0].calf, cur_legs[0].thigh),
            Leg(cur_legs[1].calf, cur_legs[1].thigh),
            Leg(cur_legs[2].calf, Leg.GET_DOWN_THIGH),
            Leg(cur_legs[3].calf, Leg.GET_DOWN_THIGH),
        ]

        self._translate(dis_legs, cur_legs)

        self._generate_wait_action(0.3)

        self._copy_legs(cur_legs, dis_legs)

        dis_legs[0].calf = cur_legs[0].calf
        dis_legs[1].calf = cur_legs[1].calf
        dis_legs[2].calf = cur_legs[2].calf
        dis_legs[3].calf = cur_legs[3].calf

        dis_legs[0].thigh = Leg.GET_DOWN_THIGH
        dis_legs[1].thigh = Leg.GET_DOWN_THIGH
        dis_legs[2].thigh = cur_legs[2].thigh
        dis_legs[3].thigh = cur_legs[3].thigh

        self._translate(dis_legs, cur_legs)

        self._generate_wait_action(0.3)

        self._copy_legs(cur_legs, dis_legs)

        dis_legs[0].calf = cur_legs[0].calf
        dis_legs[1].calf = cur_legs[1].calf
        dis_legs[2].calf = Leg.GET_DOWN_CALF
        dis_legs[3].calf = Leg.GET_DOWN_CALF

        dis_legs[0].thigh = cur_legs[0].thigh
        dis_legs[1].thigh = cur_legs[1].thigh
        dis_legs[2].thigh = cur_legs[2].thigh
        dis_legs[3].thigh = cur_legs[3].thigh

        self._translate(dis_legs, cur_legs)

        self._generate_wait_action(0.3)

        self._copy_legs(cur_legs, dis_legs)

        dis_legs[0].calf = Leg.STAND_CALF
        dis_legs[1].calf = Leg.STAND_CALF
        dis_legs[2].calf = cur_legs[2].calf
        dis_legs[3].calf = cur_legs[3].calf

        dis_legs[0].thigh = cur_legs[0].thigh
        dis_legs[1].thigh = cur_legs[1].thigh
        dis_legs[2].thigh = cur_legs[2].thigh
        dis_legs[3].thigh = cur_legs[3].thigh

        self._translate(dis_legs, cur_legs)

        self._generate_pose_action(Action.SITDOWN)

    def _stand_up(self):
        cur_legs = [
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
        ]
        if self._get_current_pose() != Action.STAND:
            self._get_down()
        else:
            self._copy_legs(cur_legs, self._get_current_legs())

        dis_legs = [
            Leg(cur_legs[0].calf, cur_legs[0].thigh),
            Leg(cur_legs[1].calf, cur_legs[1].thigh),
            Leg(cur_legs[2].calf, Leg.STAND_THIGH),
            Leg(cur_legs[3].calf, Leg.STAND_THIGH),
        ]

        self._translate(dis_legs, cur_legs)

        self._copy_legs(cur_legs, dis_legs)

        dis_legs[0].calf = cur_legs[0].calf
        dis_legs[1].calf = cur_legs[1].calf
        dis_legs[2].calf = cur_legs[2].calf
        dis_legs[3].calf = cur_legs[3].calf

        dis_legs[0].thigh = Leg.STAND_THIGH
        dis_legs[1].thigh = Leg.STAND_THIGH
        dis_legs[2].thigh = cur_legs[2].thigh
        dis_legs[3].thigh = cur_legs[3].thigh

        self._translate(dis_legs, cur_legs)

        self._copy_legs(cur_legs, dis_legs)

        dis_legs[0].calf = Leg.STAND_CALF
        dis_legs[1].calf = Leg.STAND_CALF
        dis_legs[2].calf = Leg.STAND_CALF
        dis_legs[3].calf = Leg.STAND_CALF

        dis_legs[0].thigh = cur_legs[0].thigh
        dis_legs[1].thigh = cur_legs[1].thigh
        dis_legs[2].thigh = cur_legs[2].thigh
        dis_legs[3].thigh = cur_legs[3].thigh

        self._translate(dis_legs, cur_legs)

        self._generate_pose_action(Action.STAND)

    def _get_down(self):
        cur_legs = self._get_current_legs()

        self._generate_pose_action(Action.GET_DOWN)

        dis_legs = [
            Leg(cur_legs[0].calf, cur_legs[0].thigh),
            Leg(cur_legs[1].calf, cur_legs[1].thigh),
            Leg(cur_legs[2].calf, Leg.GET_DOWN_THIGH),
            Leg(cur_legs[3].calf, Leg.GET_DOWN_THIGH),
        ]

        self._translate(dis_legs, cur_legs)

        self._generate_wait_action(0.3)

        self._copy_legs(cur_legs, dis_legs)

        dis_legs[0].calf = cur_legs[0].calf
        dis_legs[1].calf = cur_legs[1].calf
        dis_legs[2].calf = cur_legs[2].calf
        dis_legs[3].calf = cur_legs[3].calf

        dis_legs[0].thigh = Leg.GET_DOWN_THIGH
        dis_legs[1].thigh = Leg.GET_DOWN_THIGH
        dis_legs[2].thigh = cur_legs[2].thigh
        dis_legs[3].thigh = cur_legs[3].thigh

        self._translate(dis_legs, cur_legs)
        self._generate_wait_action(0.3)

        self._copy_legs(cur_legs, dis_legs)

        dis_legs[0].calf = cur_legs[0].calf
        dis_legs[1].calf = cur_legs[1].calf
        dis_legs[2].calf = Leg.GET_DOWN_CALF
        dis_legs[3].calf = Leg.GET_DOWN_CALF

        dis_legs[0].thigh = cur_legs[0].thigh
        dis_legs[1].thigh = cur_legs[1].thigh
        dis_legs[2].thigh = cur_legs[2].thigh
        dis_legs[3].thigh = cur_legs[3].thigh

        self._translate(dis_legs, cur_legs)

        self._generate_wait_action(0.3)

        self._copy_legs(cur_legs, dis_legs)

        dis_legs[0].calf = Leg.GET_DOWN_CALF
        dis_legs[1].calf = Leg.GET_DOWN_CALF
        dis_legs[2].calf = cur_legs[2].calf
        dis_legs[3].calf = cur_legs[3].calf

        dis_legs[0].thigh = cur_legs[0].thigh
        dis_legs[1].thigh = cur_legs[1].thigh
        dis_legs[2].thigh = cur_legs[2].thigh
        dis_legs[3].thigh = cur_legs[3].thigh

        self._translate(dis_legs, cur_legs)
        self._generate_wait_action(0.3)

        self._generate_pose_action(Action.GET_DOWN)

    def _generate_pose_action(self, pose):
        act = Action()
        act.type = Action.POSE_TYPE
        act.pose = pose
        self._actions.append(act)

    def _generate_wait_action(self, time):
        act = Action()
        act.type = Action.WAIT_TYPE
        act.wait_time = time
        self._actions.append(act)

    def _generate_steer_action(self, legs):
        act = Action()
        act.legs.append(Leg(0, 0))
        act.legs.append(Leg(0, 0))
        act.legs.append(Leg(0, 0))
        act.legs.append(Leg(0, 0))

        for i in range(len(legs)):
            act.legs[i].calf = legs[i].calf
            act.legs[i].thigh = legs[i].thigh
        act.type = Action.STEER_TYPE
        self._actions.append(act)
        # self._debug_print_leg(act.legs)

    def _translate(self, angle_dis, angle_cur):

        count = 0
        for i in range(len(angle_cur)):
            count = max(abs(angle_cur[i].calf - angle_dis[i].calf), count)
            count = max(abs(angle_cur[i].thigh - angle_dis[i].thigh), count)

        if count == 0:
            return

        div = []
        for i in range(len(angle_cur)):
            div.append(
                Leg(
                    (angle_cur[i].calf - angle_dis[i].calf) / count,
                    (angle_cur[i].thigh - angle_dis[i].thigh) / count,
                )
            )
        j = 0
        while j < count:
            for i in range(len(angle_cur)):
                angle_cur[i].calf -= div[i].calf
                angle_cur[i].thigh -= div[i].thigh
            self._generate_steer_action(angle_cur)
            j += 1

    def _debug_print_leg(self, legs):
        str_dbg = ""
        for i in range(len(legs)):
            str_dbg = str_dbg + str(legs[i].calf) + " " + str(legs[i].thigh) + " "
        self.get_logger().info(f"dis:{str_dbg}")

    def _usual_action(self):
        while rclpy.ok():
            time.sleep(0.01)
            act = Action()
            with self._action_lock:
                if len(self._actions) == 0:
                    continue
                else:
                    act.type = self._actions[0].type
                    act.pose = self._actions[0].pose
                    act.legs.extend(self._actions[0].legs)
                    act.wait_time = self._actions[0].wait_time

                    del self._actions[0]

            if act.type == Action.STEER_TYPE:
                with self._legs_lock:
                    self._copy_legs(self._legs, act.legs)
                    # self._debug_print_leg(act.legs)
                    # self._debug_print_leg(self._legs)
                self._set_cat_legs_angle()
                # self.get_logger().info(f"steer")
            elif act.type == Action.POSE_TYPE:
                with self._pose_lock:
                    self._pose = act.pose
                # self.get_logger().info(f"pose:{act.pose}")

            elif act.type == Action.WAIT_TYPE:
                time.sleep(act.wait_time)
                # self.get_logger().info(f"sleep:{act.wait_time}")

    def _random_action(self):
        while rclpy.ok():
            time.sleep(random.randint(5, 20))

            with self._cat_status_lock:
                if (
                    self._cat_status == CatStatus.CAT_DONT_MOVE
                    or self._cat_status == CatStatus.CAT_STOP
                ):
                    continue

            with self._action_lock:
                if len(self._actions) != 0:
                    continue
                else:
                    i = random.randint(0, 10)
                    if i == 0:
                        self._sit_down()
                    elif i == 1:
                        self._stand_up()
                    elif i == 2:
                        self._get_down()
                    elif i == 3:
                        self._foreward(random.randint(3, 5))
                    elif i == 4:
                        self._retreat(random.randint(3, 5))
                    elif i == 5:
                        with self._cat_deriction_lock:
                            if self._cat_deriction == CatDeriction.HORIZONTAL:
                                i = random.randint(0, 10)
                                if i == 3:
                                    self._turn_round()
                                elif i == 5:
                                    self._turn_left()
                                elif i == 6:
                                    self._turn_right()
                                else:
                                    pass
                            else:
                                pass

    def _set_cat_legs_angle(self):
        msg = CatSteer()
        msg.number = CatSteer.LEG_STERR_NUMBER
        msg.type = CatSteer.LEG_TYPE
        msg.steer.append(CatSteer.LEG1_THIGH)
        msg.steer.append(CatSteer.LEG1_CALF)
        msg.steer.append(CatSteer.LEG2_THIGH)
        msg.steer.append(CatSteer.LEG2_CALF)
        msg.steer.append(CatSteer.LEG3_THIGH)
        msg.steer.append(CatSteer.LEG3_CALF)
        msg.steer.append(CatSteer.LEG4_THIGH)
        msg.steer.append(CatSteer.LEG4_CALF)

        i = 0
        while i < len(self._legs):
            self._legs[i].deal_out_range_angle()
            msg.angle.append(int(self._legs[i].thigh))
            msg.angle.append(int(self._legs[i].calf))
            i += 1

        self._leg_publisher.publish(msg)

    def _compare(self, array1, array2):
        if len(array1) != len(array2):
            return False

        for i in range(len(array1)):
            if array1[i].calf != array2[i].calf or array1[i].thigh != array2[i].thigh:
                return False

        return True

    def _get_current_pose(self):
        cur_pose = Action.GET_DOWN

        with self._pose_lock:
            cur_pose = self._pose

        return cur_pose

    def _get_current_legs(self):
        cur_legs = [
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
            Leg(Leg.GET_DOWN_CALF, Leg.GET_DOWN_THIGH),
        ]
        with self._legs_lock:
            self._copy_legs(cur_legs, self._legs)

        return cur_legs

    def _copy_legs(self, dis_legs, org_legs):
        for i in range(len(dis_legs)):
            dis_legs[i].calf = org_legs[i].calf
            dis_legs[i].thigh = org_legs[i].thigh

    def _quiet_action(self):
        if random.randint(1, 10) % 2 == 1:
            self._sit_down()
        else:
            self._stand_up()


def main():
    rclpy.init()
    node = CatLegsNode("cat_legs")
    rclpy.spin(node)
    rclpy.shutdown()
